/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IMUClient.cpp,v 1.12 2009-10-23 13:56:43 morsko Exp $
 *
 * \file
 * \brief MaCI - IMU Interface Client header
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 */
#include "IMUClient.hpp"
#include "binbag.h"
#include "timestamp.hpp"
//*****************************************************************************
using namespace MaCI::IMU;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CIMUClient::CIMUClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor)
  :CSync(6,1),
   CThread(1),
   CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_IMU, aInterfaceMinor),
   iAccelerationDataSequence(-1),
   iAngularVelocityDataSequence(-1),
   iAngleDataSequence(-1),
   iQuaternionDataSequence(-1),
   iOrientationMatrixDataSequence(-1),
   iEventSequence(-1),
   iCmdSequence(0),
   iContinuousTypes(0),
   iAcceleration(),
   iAngularVelocity(),
   iAngle(),
   iQuaternion(),
   iOrientationMatrix(),
   iEvent(),
   iAccelerationBacklog(3000, 10000),
   iAngularVelocityBacklog(3000, 10000),
   iAngleBacklog(3000, 10000),   
   iQuaternionBacklog(3000, 10000),
   iOrientationMatrixBacklog(3000, 10000)

{
  dPrint(4,"CIMUClient %p constructed",this);

}
//*****************************************************************************
      
CIMUClient::~CIMUClient(){
  DoClose();
  dPrint(4,"CIMUClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CIMUClient::DoOpen(void)
{
  MaCI::EMaCIError result = KMaCIOK;

  // Checks.
  if(!IsActive()) {

    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if (result == KMaCIOK) {
   
      ///Get event type
      if(!GetEventTypes()){
        dPrint(3,"Failed to get event types from server");
        return KMaCIError ;
      }

      // All OK, so start the Thread
      SetActive(true);
      CThread::RunThread();
    }
  }else{
    dPrint(1, "No GIMIPtr - No Provided service!");
    result = KMaCIError;

  }
  //Unlock();
  return result;



}
//*****************************************************************************

MaCI::EMaCIError CIMUClient::DoClose(void)
{
  MaCI::EMaCIError result = KMaCIError;
  if(IsActive()) {
    SetActive(false);

    // Wait for thread to complete.
    CThread::SetRequestTermination();
    /* CThread::CancelThread(); //Removed by script */
    
    result = UnsubscribeToInterface(TimeoutValue(2000));

    //Notify all
    Notify(KTypeAcceleration3d);
    Notify(KTypeAngularVelocity3d);
    Notify(KTypeAngle3d);
    Notify(KTypeQuaternion);
    Notify(KTypeOrientationMatrix);

    CThread::WaitThread();
    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed IMUClient");    
    }else{
      dPrint(4,"Unsubscribing failed at IMUClient!");
    }
    
    
  } else {
    dPrint(4, "DummyClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

int CIMUClient::ThreadFunction(const int aThreadNumber)
{
  int p = 0;
  int result = 0;
  int r;
  gimi::GIMIMessage gmsg;

  assert(aThreadNumber == 0);
  
  dPrint(3,"GIMIMessage handler started");

  // Run as long as the thread should be active
  while(IsActive()) {
    r = iGIMIPtr->receive(gmsg,
			  1000*60*60*24,
			  iInterfaceMajor,
			  iInterfaceMinor,
			  iRedirectionBufferId);
    switch(r) {
    case GIMI_OK:
      dPrint(10,"GOT GIMI MSG");
     
      // Got GIMI OK, so process message.
      p = ProcessMessage(gmsg);
      break;

    case GIMI_TIMEOUT:
      // GIMI Timeout - Do nothing.
      break;

    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(2,"GIMI Error (%d, '%s') received. Sleeping one second.",
	     r, gimi::getGimiErrorString(r).c_str());
      if(IsActive()){
        ownSleep_ms(1000);
      }

    }
    TestCancel();
  }

  dPrint(3,"GIMIMessage handler stopped");

  return result;



}
//*****************************************************************************

int CIMUClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CIMUData imuData;
  bool r;
  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    
    imuData.Reset();
    r =  imuData.DecodeFrom(&bb);
    if (r) {
      
      //positionData.Print(20);
      const TCommand *com = imuData.GetCommand();
      if(com){
        switch(com->cmd){
        case KCommandEvent:{
	  dPrint(10,"GOT EVENT");
          for(unsigned int i = 0; i<10; i++){
            if(((iContinuousTypes>>i) && 1) == 1){
              switch(i){
              case KTypeAcceleration3d:{
                Lock();
                if(imuData.GetAcceleration3d()){
                  iAcceleration = imuData;
                  iAccelerationDataSequence++;
                  
                  if(imuData.GetTimestamp()){
                    iAccelerationBacklog.PushData(imuData.GetTimestamp()->GetGimTime(),
                                                  imuData,
                                                  false);
                    
                  }
                  Notify(KTypeAcceleration3d);
                }
                Unlock();
                break;
              }
              case KTypeAngularVelocity3d:{
                Lock();
                if(imuData.GetAngularVelocity3d()){
                  iAngularVelocity = imuData;
                  iAngularVelocityDataSequence++;
                  if(imuData.GetTimestamp()){
                    iAngularVelocityBacklog.PushData(imuData.GetTimestamp()->GetGimTime(),
                                              imuData,
                                              false);
                    
                  }
                  Notify(KTypeAngularVelocity3d);
                }
                Unlock();
                break;
              }
              case KTypeAngle3d:{
                Lock();
                if(imuData.GetAngle3d()){
                  iAngle = imuData;
                  iAngleDataSequence++;
                  if(imuData.GetTimestamp()){
                    iAngleBacklog.PushData(imuData.GetTimestamp()->GetGimTime(),
                                                imuData,
                                           false);
                    
                  }
                  Notify(KTypeAngle3d);
                }
                Unlock();
                break;
              }
              case KTypeQuaternion:{
                Lock();
                if(imuData.GetQuaternion()){
                  iQuaternion = imuData;
                  iQuaternionDataSequence++;
                  if(imuData.GetTimestamp()){
                    iQuaternionBacklog.PushData(imuData.GetTimestamp()->GetGimTime(),
                                                imuData,
                                                false);
                    
                  }
                  Notify(KTypeQuaternion);
                }
                Unlock();
                break;
              }
              case KTypeOrientationMatrix:{
                Lock();
                if(imuData.GetOrientationMatrix()){
                  iOrientationMatrix = imuData;
                  iOrientationMatrixDataSequence++;
                  if(imuData.GetTimestamp()){
                    iOrientationMatrixBacklog.PushData(imuData.GetTimestamp()->GetGimTime(),
                                                       imuData,
                                                       false);
                    
                  }
                  Notify(KTypeOrientationMatrix);
                }
                Unlock();
                break;
              }

              }
            }
          }

          /* Lock();
	  iEvent = imuData;
          for(unsigned int i = 0; i<10; i++){
            if(((iContinuousTypes>>i) && 1) == 1){
              Notify(i);
              
            }
          }
	  Unlock();
          break;*/
          break;
        }

        case KCommandGetAcceleration3dReply:{
          dPrint(10,"GET acceleration reply");
          Lock();
          iAcceleration = imuData;
          iAccelerationDataSequence++;
          Notify(KTypeAcceleration3d);
          Unlock();          
          break;
        }
        case KCommandGetAngularVelocity3dReply :{
          dPrint(10,"GET Angular Velocity reply");
          Lock();
          iAngularVelocity = imuData;
          iAngularVelocityDataSequence++;
          Notify(KTypeAngularVelocity3d);
          Unlock();          
          break;
        }
        case KCommandGetAngle3dReply :{
          dPrint(10,"GET angle reply");
          Lock();
          iAngle = imuData;
          iAngleDataSequence++;
          Notify(KTypeAngle3d);
          Unlock();          
          break; 
       }
        case KCommandGetQuaternionReply :{
          dPrint(10,"GET quaternion reply");
          Lock();
          iQuaternion = imuData;
          iQuaternionDataSequence++;
          Notify(KTypeQuaternion);
          Unlock();          
          break;
        }
        case KCommandGetOrientationMatrixReply:{
          dPrint(10,"GET o-matrix reply");
          Lock();
          iOrientationMatrix = imuData;
          iOrientationMatrixDataSequence++;
          Notify(KTypeOrientationMatrix);
          Unlock();          
          break;
        }
        default:{
          dPrint(5,"This message is in a wrong place :(,type 0x%x",com->cmd);
          break;
        }

        }

      }
    }
  }
  return result;


}
//*****************************************************************************

bool CIMUClient::GetAccelerations(TAcceleration3d &aAcceleration,
                                  MaCI::Common::TTimestamp &aTimestamp, 
                                  int *aSeq, 
                                  int aTimeout_ms)
{
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;
  dPrint(6,"Getting acceleration iContinuousTypes %u, %d", iContinuousTypes,(iContinuousTypes >> KTypeAcceleration3d) && 1);
 
  if(((iContinuousTypes >> KTypeAcceleration3d) && 1) != 1){
    //Not continuous type, have to poll for the data

    int r;
    int messageId = 0;
    CIMUData imuData;
    gim::binbag::CBinBag bb;
    imuData.EncodeTo(&bb);
    
    imuData.SetCommand(TCommand(KCommandGetAcceleration3d));
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get accelerations");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);

    switch(r) {
    case GIMI_OK:{
      break;
    }
    case GIMI_NOCONNECTION:{
      dPrint(3,"NO CONNECTION ");
      result = false;
        return false;
      break;
    }
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      result =false;
      return false;
      break;
    }

   ;
  }

  Lock();
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    if(iAcceleration.GetAcceleration3d()){
      aAcceleration.accelerationX = iAcceleration.GetAcceleration3d()->accelerationX;
      aAcceleration.accelerationY = iAcceleration.GetAcceleration3d()->accelerationY;
      aAcceleration.accelerationZ = iAcceleration.GetAcceleration3d()->accelerationZ;
      result = true;
    }
    if(iAcceleration.GetTimestamp()){
      aTimestamp = *(iAcceleration.GetTimestamp());

    }
    
  } else {
    
    if (iAccelerationDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KTypeAcceleration3d);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
        
      } while(IsActive() && iAccelerationDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iAccelerationDataSequence > *aSeq) {
        // Yes :)
        if(iAcceleration.GetAcceleration3d()){
          aAcceleration.accelerationX = iAcceleration.GetAcceleration3d()->accelerationX;
          aAcceleration.accelerationY = iAcceleration.GetAcceleration3d()->accelerationY;
          aAcceleration.accelerationZ = iAcceleration.GetAcceleration3d()->accelerationZ;
        }
        *aSeq = iAccelerationDataSequence;
        if(iAcceleration.GetTimestamp()){
          aTimestamp = *(iAcceleration.GetTimestamp());
          
        }
        result = true;

        
      } else {
        // No :(
        dPrint(3, "Failed to get Acceleration! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      if(iAcceleration.GetAcceleration3d()){
        aAcceleration.accelerationX = iAcceleration.GetAcceleration3d()->accelerationX;
        aAcceleration.accelerationY = iAcceleration.GetAcceleration3d()->accelerationY;
        aAcceleration.accelerationZ = iAcceleration.GetAcceleration3d()->accelerationZ;
      }
      *aSeq = iAccelerationDataSequence;
      if(iAcceleration.GetTimestamp()){
        aTimestamp = *(iAcceleration.GetTimestamp());
        
      }
      result = true;
    }
  }
  Unlock();
  
  return result;
  
}
//*****************************************************************************

bool CIMUClient::GetAccelerations(TAcceleration3d &aAcceleration,
                                  MaCI::Common::TTimestamp &aTimestamp,
                                  MaCI::Common::TTimestamp &aTime, 
                                  gim::time *aTimediff)
{
  bool result = false;
  CIMUData acc;
  result = iAccelerationBacklog.PeekData(acc, aTime.GetGimTime(), aTimediff);

  if(result && iAcceleration.GetAcceleration3d()){
    aAcceleration.accelerationX = acc.GetAcceleration3d()->accelerationX;
    aAcceleration.accelerationY = acc.GetAcceleration3d()->accelerationY;
    aAcceleration.accelerationZ = acc.GetAcceleration3d()->accelerationZ;
  }else{
    result = false;
  }

  if(result && iAcceleration.GetTimestamp()){
    aTimestamp = *(acc.GetTimestamp());

  }
  return result ;
}
//*****************************************************************************

bool CIMUClient::GetAngularVelocities(TAngularVelocity3d &aAngularVelocity,
                              MaCI::Common::TTimestamp &aTimestamp,
                              int *aSeq,
                              int aTimeout_ms)
{
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;
  

  if(((iContinuousTypes >> KTypeAngularVelocity3d) && 1) != 1){
    //Not continuous type, have to poll for the data

    int r;
    int messageId = 0;
    CIMUData imuData;
    gim::binbag::CBinBag bb;
    r = imuData.EncodeTo(&bb);
    
    imuData.SetCommand(TCommand(KCommandGetAngularVelocity3d));
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get Angular Velocity ");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);

    switch(r) {
    case GIMI_OK:{

      break;
    }
    case GIMI_NOCONNECTION:{
      dPrint(3,"NO CONNECTION ");
      result = false;
        return false;
      break;
    }
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      result =false;
      return false;
      break;
    }

   
  }

  Lock();
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    aAngularVelocity.wX = iAngularVelocity.GetAngularVelocity3d()->wX;
    aAngularVelocity.wY = iAngularVelocity.GetAngularVelocity3d()->wY;
    aAngularVelocity.wZ = iAngularVelocity.GetAngularVelocity3d()->wZ;
    if(iAngularVelocity.GetTimestamp()){
        aTimestamp = *(iAngularVelocity.GetTimestamp());
        
    }
    result = true;
    
  } else {
    
    if (iAngularVelocityDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KTypeAngularVelocity3d);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
        
      } while(IsActive() && iAngularVelocityDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iAngularVelocityDataSequence > *aSeq) {
        // Yes :)
        aAngularVelocity.wX = iAngularVelocity.GetAngularVelocity3d()->wX;
        aAngularVelocity.wY = iAngularVelocity.GetAngularVelocity3d()->wY;
        aAngularVelocity.wZ = iAngularVelocity.GetAngularVelocity3d()->wZ;
        *aSeq = iAngularVelocityDataSequence;
        if(iAngularVelocity.GetTimestamp()){
          aTimestamp = *(iAngularVelocity.GetTimestamp());
          
        }
        result = true;

        
      } else {
        // No :(
        dPrint(3, "Failed to get AngularVelocity! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      aAngularVelocity.wX = iAngularVelocity.GetAngularVelocity3d()->wX;
      aAngularVelocity.wY = iAngularVelocity.GetAngularVelocity3d()->wY;
      aAngularVelocity.wZ = iAngularVelocity.GetAngularVelocity3d()->wZ;
      *aSeq = iAngularVelocityDataSequence;
      if(iAngularVelocity.GetTimestamp()){
        aTimestamp = *(iAngularVelocity.GetTimestamp());
        
      }
      result = true;
    }
  }
  Unlock();
  
  return result;
}
//*****************************************************************************

bool CIMUClient::GetAngularVelocities(TAngularVelocity3d &aAngularVelocity,
                              MaCI::Common::TTimestamp &aTimestamp,
                              MaCI::Common::TTimestamp &aTime, 
                              gim::time *aTimediff)
{
  bool result = false;
  CIMUData angV;
  result = iAngularVelocityBacklog.PeekData(angV, aTime.GetGimTime(), aTimediff);

  if(result && angV.GetAcceleration3d()){
    aAngularVelocity.wX = angV.GetAngularVelocity3d()->wX;
    aAngularVelocity.wY = angV.GetAngularVelocity3d()->wY;
    aAngularVelocity.wZ = angV.GetAngularVelocity3d()->wZ;
  }else{
    result = false;
  }

  if(result && angV.GetTimestamp()){
    aTimestamp = *(angV.GetTimestamp());

  }
  return result;
  
  
}
//*****************************************************************************

bool CIMUClient::GetAngles(TAngle3d &aAngle,
                           MaCI::Common::TTimestamp &aTimestamp,
                           int *aSeq,
                           int aTimeout_ms)
{
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;
  
 
  if(((iContinuousTypes >> KTypeAngle3d) && 1) != 1){
    //Not continuous type, have to poll for the data
      
    int r;
    int messageId = 0;
    CIMUData imuData;
    gim::binbag::CBinBag bb;
    r = imuData.EncodeTo(&bb);
    
    imuData.SetCommand(TCommand(KCommandGetAngle3d));
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get  angles");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);

    switch(r) {
    case GIMI_OK:{
      break;
    }
    case GIMI_NOCONNECTION:{
      dPrint(3,"NO CONNECTION ");
      result = false;
        return false;
      break;
    }
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      result =false;
      return false;
      break;
    }

   ;
  }

  Lock();
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    aAngle.roll = iAngle.GetAngle3d()->roll;
    aAngle.pitch = iAngle.GetAngle3d()->pitch;
    aAngle.yaw = iAngle.GetAngle3d()->yaw;
    if(iAngle.GetTimestamp()){
        aTimestamp = *(iAngle.GetTimestamp());
        
    }
    result = true;
    
  } else {
    
    if (iAngleDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KTypeAngle3d);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
        
      } while(IsActive() && iAngleDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iAngleDataSequence > *aSeq) {
        // Yes :)
        aAngle.roll = iAngle.GetAngle3d()->roll;
        aAngle.pitch = iAngle.GetAngle3d()->pitch;
        aAngle.yaw = iAngle.GetAngle3d()->yaw;
        *aSeq = iAngleDataSequence;
        if(iAngle.GetTimestamp()){
          aTimestamp = *(iAngle.GetTimestamp());
        
        }
        result = true;

        
      } else {
        // No :(
        dPrint(3, "Failed to get Angles! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      aAngle.roll = iAngle.GetAngle3d()->roll;
      aAngle.pitch = iAngle.GetAngle3d()->pitch;
      aAngle.yaw = iAngle.GetAngle3d()->yaw;
      
      *aSeq = iAngleDataSequence;
      if(iAngle.GetTimestamp()){
        aTimestamp = *(iAngle.GetTimestamp());
        
      }
      result = true;
    }
  }
  Unlock();
  
  return result;

}
//*****************************************************************************

bool CIMUClient::GetAngles(TAngle3d &aAngle,
                           MaCI::Common::TTimestamp &aTimestamp,
                           MaCI::Common::TTimestamp &aTime, 
                           gim::time *aTimediff)
{
  bool result = false;
  CIMUData ang;
  result = iAngleBacklog.PeekData(ang, aTime.GetGimTime(), aTimediff);

  if(result && ang.GetAngle3d()){
    aAngle.roll = ang.GetAngle3d()->roll;
    aAngle.pitch = ang.GetAngle3d()->pitch;
    aAngle.yaw = ang.GetAngle3d()->yaw;
    
  }else{
    result = false;
  }

  if(result && ang.GetTimestamp()){
    aTimestamp = *(ang.GetTimestamp());

  }
  return result;


}
//*****************************************************************************

bool CIMUClient::GetQuaternion(TQuaternion &aQuaternion, 
                               MaCI::Common::TTimestamp &aTimestamp,
                               int *aSeq, 
                               int aTimeout_ms)
{
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;
  
 
  if(((iContinuousTypes >> KTypeQuaternion) && 1) != 1){
    //Not continuous type, have to poll for the data
      
    int r;
    int messageId = 0;
    CIMUData imuData;
    gim::binbag::CBinBag bb;
    r = imuData.EncodeTo(&bb);
    
    imuData.SetCommand(TCommand(KCommandGetQuaternion));
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get  quaternion");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);

    switch(r) {
    case GIMI_OK:{
      break;
    }
    case GIMI_NOCONNECTION:{
      dPrint(3,"NO CONNECTION ");
      result = false;
        return false;
      break;
    }
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      result =false;
      return false;
      break;
    }

   ;
  }

  Lock();
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    aQuaternion.scalar = iQuaternion.GetQuaternion()->scalar;
    aQuaternion.x = iQuaternion.GetQuaternion()->x;
    aQuaternion.y = iQuaternion.GetQuaternion()->y;
    aQuaternion.z = iQuaternion.GetQuaternion()->z;

    if(iQuaternion.GetTimestamp()){
      aTimestamp = *(iQuaternion.GetTimestamp());
        
    }
    result = true;
    
  } else {
    
    if (iQuaternionDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KTypeQuaternion);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
        
      } while(IsActive() && iQuaternionDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iQuaternionDataSequence > *aSeq) {
        // Yes :)
        aQuaternion.scalar = iQuaternion.GetQuaternion()->scalar;
        aQuaternion.x = iQuaternion.GetQuaternion()->x;
        aQuaternion.y = iQuaternion.GetQuaternion()->y;
        aQuaternion.z = iQuaternion.GetQuaternion()->z;
        
        *aSeq = iQuaternionDataSequence;
        if(iQuaternion.GetTimestamp()){
          aTimestamp = *(iQuaternion.GetTimestamp());
        
        }
        result = true;

        
      } else {
        // No :(
        dPrint(3, "Failed to get Quaternion! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      aQuaternion.scalar = iQuaternion.GetQuaternion()->scalar;
      aQuaternion.x = iQuaternion.GetQuaternion()->x;
      aQuaternion.y = iQuaternion.GetQuaternion()->y;
      aQuaternion.z = iQuaternion.GetQuaternion()->z;
      
      *aSeq = iQuaternionDataSequence;
      if(iQuaternion.GetTimestamp()){
        aTimestamp = *(iQuaternion.GetTimestamp());
        
      }
      result = true;
    }
  }
  Unlock();
  
  return result;

}
//*****************************************************************************
 
bool CIMUClient::GetQuaternion(TQuaternion &aQuaternion,
                               MaCI::Common::TTimestamp &aTimestamp,
                               MaCI::Common::TTimestamp &aTime, 
                               gim::time *aTimediff)
{
  bool result = false;
  CIMUData quaternion;
  result = iQuaternionBacklog.PeekData(quaternion, aTime.GetGimTime(), aTimediff);

  if(result && quaternion.GetQuaternion()){
    aQuaternion.scalar = quaternion.GetQuaternion()->scalar;
    aQuaternion.x = quaternion.GetQuaternion()->x;
    aQuaternion.y = quaternion.GetQuaternion()->y;
    aQuaternion.z = quaternion.GetQuaternion()->z;
   
  }else{
    result = false;
  }

  if(result && quaternion.GetTimestamp()){
    aTimestamp = *(quaternion.GetTimestamp());

  }
  return result;


}
//*****************************************************************************

bool CIMUClient::GetOrientationMatrix(TOrientationMatrix &aOrientationMatrix,
                                      MaCI::Common::TTimestamp &aTimestamp,
                                      int *aSeq, 
                                      int aTimeout_ms)
{
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;
  
 
  if(((iContinuousTypes >> KTypeOrientationMatrix) && 1) != 1){
    //Not continuous type, have to poll for the data
      
    int r;
    int messageId = 0;
    CIMUData imuData;
    gim::binbag::CBinBag bb;
    r = imuData.EncodeTo(&bb);
    
    imuData.SetCommand(TCommand(KCommandGetOrientationMatrix));
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get  orientation matrix");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);

    switch(r) {
    case GIMI_OK:{
      break;
    }
    case GIMI_NOCONNECTION:{
      dPrint(3,"NO CONNECTION ");
      result = false;
        return false;
      break;
    }
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      result =false;
      return false;
      break;
    }

   ;
  }

  Lock();
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync

    const TOrientationMatrix *om = iOrientationMatrix.GetOrientationMatrix();
    aOrientationMatrix.e11 = om->e11;
    aOrientationMatrix.e12 = om->e12;
    aOrientationMatrix.e13 = om->e13;
    aOrientationMatrix.e21 = om->e21;
    aOrientationMatrix.e22 = om->e22;
    aOrientationMatrix.e23 = om->e23;
    aOrientationMatrix.e31 = om->e31;
    aOrientationMatrix.e32 = om->e32;
    aOrientationMatrix.e33 = om->e33;
    if(iOrientationMatrix.GetTimestamp()){
      aTimestamp = *(iOrientationMatrix.GetTimestamp());
        
    }
    result = true;
    
  } else {
    
    if (iOrientationMatrixDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KTypeOrientationMatrix);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
        
      } while(IsActive() && iOrientationMatrixDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iOrientationMatrixDataSequence > *aSeq) {
        // Yes :)
        const TOrientationMatrix *om = iOrientationMatrix.GetOrientationMatrix();
        aOrientationMatrix.e11 = om->e11;
        aOrientationMatrix.e12 = om->e12;
        aOrientationMatrix.e13 = om->e13;
        aOrientationMatrix.e21 = om->e21;
        aOrientationMatrix.e22 = om->e22;
        aOrientationMatrix.e23 = om->e23;
        aOrientationMatrix.e31 = om->e31;
        aOrientationMatrix.e32 = om->e32;
        aOrientationMatrix.e33 = om->e33;
       
        *aSeq = iOrientationMatrixDataSequence;
        if(iOrientationMatrix.GetTimestamp()){
          aTimestamp = *(iOrientationMatrix.GetTimestamp());
        
        }
        result = true;

        
      } else {
        // No :(
        dPrint(3, "Failed to get Orientation matrix! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      const TOrientationMatrix *om = iOrientationMatrix.GetOrientationMatrix();
      aOrientationMatrix.e11 = om->e11;
      aOrientationMatrix.e12 = om->e12;
      aOrientationMatrix.e13 = om->e13;
      aOrientationMatrix.e21 = om->e21;
      aOrientationMatrix.e22 = om->e22;
      aOrientationMatrix.e23 = om->e23;
      aOrientationMatrix.e31 = om->e31;
      aOrientationMatrix.e32 = om->e32;
      aOrientationMatrix.e33 = om->e33;
      *aSeq = iOrientationMatrixDataSequence;
      if(iOrientationMatrix.GetTimestamp()){
        aTimestamp = *(iOrientationMatrix.GetTimestamp());
        
      }
      result = true;
    }
  }
  Unlock();
  
  return result;

}
//*****************************************************************************

bool CIMUClient::GetOrientationMatrix(TOrientationMatrix &aOrientationMatrix,
                                      MaCI::Common::TTimestamp &aTimestamp,
                                      MaCI::Common::TTimestamp &aTime, 
                                      gim::time *aTimediff)
{

  bool result = false;
  CIMUData orientationMatrix;
  result = iOrientationMatrixBacklog.PeekData(orientationMatrix, aTime.GetGimTime(), aTimediff);
  const TOrientationMatrix *om =  orientationMatrix.GetOrientationMatrix();
  if(result && om){
    
    aOrientationMatrix.e11 = om->e11;
    aOrientationMatrix.e12 = om->e12;
    aOrientationMatrix.e13 = om->e13;
    aOrientationMatrix.e21 = om->e21;
    aOrientationMatrix.e22 = om->e22;
    aOrientationMatrix.e23 = om->e23;
    aOrientationMatrix.e31 = om->e31;
    aOrientationMatrix.e32 = om->e32;
    aOrientationMatrix.e33 = om->e33;
    
  }else{
    result = false;
  }

  if(result && orientationMatrix.GetTimestamp()){
    aTimestamp = *(orientationMatrix.GetTimestamp());

  }
  return result;

 
}
//*****************************************************************************

bool CIMUClient::GetEventTypes()
{
  bool result;
  int r;
  int messageId = 0;
  CIMUData imuData;
  gim::binbag::CBinBag bb;
  r = imuData.EncodeTo(&bb);
  gimi::GIMIMessage gmsg;
  imuData.SetCommand(TCommand(KCommandGetEventTypes));
  Lock();
  messageId = ++iCmdSequence;
  Unlock();
    // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get event types");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
                     messageId);

  switch(r) {
  case GIMI_OK:{

    r = iGIMIPtr->receive(gmsg,
			  1000,
			  iInterfaceMajor,
			  iInterfaceMinor,
			  iRedirectionBufferId);
    switch(r) {
    case GIMI_OK:{
      // Got GIMI OK, so save the 
      gim::binbag::CBinBag bbReply;
      CIMUData imuDataReply;
    
      result = bbReply.Decode(gmsg.getData(), gmsg.getDatalength());
      if (result) {
        // Succesfully decoded as BinBag.
        imuDataReply.Reset();
        result =  imuDataReply.DecodeFrom(&bbReply);
        if (result) {

          // Get timestamp element
          const TEventTypes *et = imuDataReply.GetEventTypes();
          if (et != NULL) {
            // If the element is found, convert it to gim::time

            iContinuousTypes= et->continuousTypes;
	    dPrint(1,"Saved continuos type... %u",iContinuousTypes);
          }else{
            dPrint(3,"Failed to get eventtypes from reply");
            return false;
          }
        }else{
          dPrint(3,"Failed to decode from binbag. Reply reading failed");
          return false;
        }
      }else{
        dPrint(3,"Failed to decode reply message to binbag");
        return false;
      }
      break;
    }
    case GIMI_TIMEOUT:
      // GIMI Timeout - Do nothing.
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(2,"GIMI Error (%d, '%s') received. Sleeping one second.",
	     r, gimi::getGimiErrorString(r).c_str());
      ownSleep_ms(1000);
      break;
    }
    break;
  }
  case GIMI_NOCONNECTION:{
    dPrint(3,"NO CONNECTION ");
   ;
    return false;
      break;
  }
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
   
    return false;
    break;
  }
 
  
  
  return true;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************




